Advance Lane Lines

%matplotlib inline 

import numpy as np
import cv2 
import glob 
import matplotlib.pyplot as plt
import pickle
import matplotlib.image as mpimg 
import os

from moviepy.editor import VideoFileClip
from IPython.display import display, Markdown, HTML

camera calibration

# https://stackoverflow.com/questions/36013063/what-is-the-purpose-of-meshgrid-in-python-numpy

objp = np.zeros(shape=(6*9, 3), dtype=np.float32) # 2D rows(6*9)*cols(3)
objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)
print(objp.shape)
print(objp[5]) 

(54, 3)
[5. 0. 0.]

def camera_calibration(images):
    
    objpoints = [] # 3D Real World
    imgpoints = [] # 2D Image Plane
    images_cal = []
    
    for idx, file in enumerate(images):
        img = cv2.imread(file)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        img_size = img.shape[1], img.shape[0]
    
        ret, corners = cv2.findChessboardCorners(gray, (9,6))
        if ret:
            imgpoints.append(corners)
            objpoints.append(objp) 
            cv2.drawChessboardCorners(img, (9,6), corners, ret)
            images_cal.append(img)
            
    return objpoints, imgpoints, images_cal, img_size

images = glob.glob('./data/camera_cal/calibration*.jpg')

objpoints, imgpoints, images_cal, img_size = camera_calibration(images)

def plot_images(images, rows, cols, figsize=(12, 6)):
    fig, axes = plt.subplots(rows, cols, figsize=figsize)
    idx = 0
    for r in range(rows):
        for c in range(cols):
            ax = axes[r, c]
            ax.imshow(images[idx])
            idx += 1
    plt.tight_layout()

plot_images(images_cal, 4, 4, (15, 8))
plt.savefig('./data/output_images/images_calibrated.jpg')

Pickle the Calibration Matrices

file_pkl = f'./data/calibration_pickle.p'

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img_size, None, None)

cal_pkl = {'mtx': mtx, 'dist': dist}
with open(file_pkl, 'wb') as fp:  
    pickle.dump(cal_pkl, fp)

Test Images

test_images = glob.glob('./data/test_images/*.jpg')
#print(len(test_images))
test_images = [mpimg.imread(img) for img in test_images]
plot_images(test_images, 2, 4, figsize=(12, 3))
plt.savefig('./data/output_images/images_test_original.jpg')

Image Functions

import plotly
import plotly.express as px

fig = px.imshow(test_images[1]) # Select that has lines straight
#print(test_images[1].shape[1], test_images[1].shape[0])
fig.write_image('./data/output_images/image_PerspectiveTransform.jpg') 
fig.show()

Undistort and Warp Functions

def undistort(img):
    with open(file_pkl, 'rb') as fp:
        cal_pkl = pickle.load(fp)
    mtx = cal_pkl['mtx']
    dist = cal_pkl['dist']
    img = cv2.undistort(img, mtx, dist, None, mtx)
    return img

def warp(img):
    
    img_size = w, h = img.shape[1], img.shape[0] 
    
    #w, h = 1280, 720
    src= [[270, 674], [530, 494], [726, 494], [1044, 672]]
    dst = [[300, h], [300, 0], [w-300, 0], [w-300, h]]
    
    src = np.float32(src)
    dst = np.float32(dst)
        
    M = cv2.getPerspectiveTransform(src, dst)
    Minv = cv2.getPerspectiveTransform(dst, src)  
    
    warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)  
    unperspective = cv2.warpPerspective(warped, Minv, img_size, flags=cv2.INTER_LINEAR)
    
    return warped, unperspective, M, Minv

Threshold Functions

def grayscale(img):
    #img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    return img 

def gaussian_blur(img, ksize):

    img = cv2.GaussianBlur(img, (ksize, ksize), cv2.BORDER_DEFAULT)
    return img 

def threshold_x(img, ksize=3, thresh=(0, 255)):
    img = grayscale(img)
    
    sobelx = cv2.Sobel(img, cv2.CV_64F, 1, 0, ksize=ksize) # x-derivative
    
    sobelx_abs = np.absolute(sobelx)
    sobelx_scaled = np.uint8(sobelx_abs*255/np.max(sobelx_abs)) # 8-bit value

    q = (sobelx_scaled >= thresh[0]) & (sobelx_scaled <= thresh[1])
    
    sxbinary = np.zeros_like(sobelx_scaled)
    sxbinary[q] = 1
    
    return sxbinary

# Sobel x and y, then computes the magnitude of the gradient and applies a threshold
def threshold_mag(img, ksize, thresh=(0, 255)):
    img = grayscale(img)
    
    sobelx = cv2.Sobel(img, cv2.CV_64F, 1, 0, ksize=ksize) # x-derivative
    sobely = cv2.Sobel(img, cv2.CV_64F, 0, 1, ksize=ksize) # y-derivative
    
    # Calculate the gradient magnitude
    mag_gradient = np.sqrt(sobelx**2 + sobely**2)
    
    # Rescale to 8-bit
    scale_factor = np.max(mag_gradient) / 255
    mag_gradient = mag_gradient / scale_factor
    mag_gradient = mag_gradient.astype(np.uint8)

    q = (mag_gradient >= thresh[0]) & (mag_gradient <= thresh[1])
    
    binary_output = np.zeros_like(mag_gradient)
    binary_output[q] = 1
    
    return binary_output

# Direction Threshold
def threshold_dir(img, ksize, thresh=(0, np.pi/2)):
    img = grayscale(img)
    
    sobelx = cv2.Sobel(img, cv2.CV_64F, 1, 0, ksize=ksize) # x-derivative
    sobely = cv2.Sobel(img, cv2.CV_64F, 0, 1, ksize=ksize) # y-derivative
    
    # Calculate the gradient magnitude
    abs_sobelx = np.absolute(sobelx)
    abs_sobely = np.absolute(sobely)
    
    dir_grad = np.arctan2(abs_sobely, abs_sobelx)
    q = (dir_grad >= thresh[0]) & (dir_grad <= thresh[1])
    
    binary_output = np.zeros_like(dir_grad)
    binary_output[q] = 1
    
    return binary_output

# Hue, Saturation, Value
def threshold_hsv(img, thresh):
    hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV)
    
    q1 = (hsv[:,:,0] >= thresh[0]) & (hsv[:,:,0] <= thresh[1])
    q2 = (hsv[:,:,1] >= thresh[0]) & (hsv[:,:,1] <= thresh[1])
    q3 = (hsv[:,:,2] >= thresh[0]) & (hsv[:,:,2] <= thresh[1])
    q = q1 & q2 & q3
    
    color_binary = np.zeros_like(img)
    color_binary[q] = 1
    
    return color_binary

# Hue, Saturation, Lightness
def threshold_hls(img, thresh):
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)  
    
    s = hls[:, :, 2]
    q = (s >= thresh[0]) & (s <= thresh[1])
    
    s_binary = np.zeros_like(s)
    s_binary[q] = 1
    
    return s_binary    
    

Viz Plot Functions

def plot_pair_images(images1, images2, rows, cols, figsize=(12, 6), ttl1='', ttl2=''):
    fig, axes = plt.subplots(rows, cols*2, figsize=figsize)
    idx = 0
    for r in range(rows):
        for c in range(0, cols*2, 2):
            ax1 = axes[r, c]
            img1 = images1[idx]
            ax1.imshow(img1)
            ax1.set_title(ttl1)
            
            ax2 = axes[r, c+1]
            img2 = images2[idx]
            ax2.imshow(img2)
            ax2.set_title(ttl2)
            
            idx += 1 
    plt.tight_layout()

def plot_5_images(imgs1, imgs2, imgs3, imgs4, imgs5, imgs6, t1, t2, t3, t4, t5, t6, figsize=(15,10)):
    imgs = [imgs1, imgs2, imgs3, imgs4, imgs5, imgs6]
    ttl = [t1, t2, t3, t4, t5, t6]
    
    fig, axes = plt.subplots(len(imgs1), 6, figsize=figsize)
    
    idx = 0
    for r in range(len(imgs1)):
        for c in range(6):
            img = imgs[c][idx]
            if (len(imgs1)==1):
                ax = axes[c]
            else:
                ax = axes[r, c]
            if c <= 1:
                ax.imshow(img)
            else:
                ax.imshow(img, cmap='gray')
            ax.set_title(ttl[c])            
        idx += 1 

def plot_pair_images_fit(images1, images2, rows, cols, figsize=(12, 6), ttl1='', ttl2='', imagesY=None, imagesLX=None, imagesRX=None):
    fig, axes = plt.subplots(rows, cols*2, figsize=figsize)
    idx = 0
    for r in range(rows):
        for c in range(0, cols*2, 2):
            ax1 = axes[r, c]
            img1 = images1[idx]
            ax1.imshow(img1)
            ax1.set_title(ttl1)
            
            ax2 = axes[r, c+1]
            img2 = images2[idx]
            ax2.imshow(img2)
            
            if imagesY:
                ax2.plot(imagesLX[idx], imagesY[idx], color='yellow')
                ax2.plot(imagesRX[idx], imagesY[idx], color='yellow') 
                
            ax2.set_title(ttl2)
            
            idx += 1 
    plt.tight_layout()

Undistort Images

Calibration Images - Undistorted

calibration_images = glob.glob('./data/camera_cal/calibration*.jpg')
calibration_images = [mpimg.imread(img) for img in calibration_images]

calibration_images_undist = [undistort(img) for img in calibration_images]
len(calibration_images_undist)

20

plot_pair_images(calibration_images, calibration_images_undist, 10, 2, (15, 25), 'original', 'undistorted')

plt.savefig('./data/output_images/calibration_images_undistorted.jpg') 

Test Images - Undistorted

test_images = glob.glob('./data/test_images/*.jpg')
test_images = [mpimg.imread(img) for img in test_images]

test_images_undist = [undistort(img) for img in test_images]
len(test_images_undist)

8

plot_pair_images(test_images, test_images_undist, 4, 2, (15, 10), 'original', 'undistorted')

plt.savefig('./data/output_images/test_images_undistorted.jpg') 

Threshold Image

images = glob.glob('./data/test_images/*.jpg')
image = images[2] 
image = mpimg.imread(image)
image = undistort(image)
image = gaussian_blur(image, ksize=5)

#image = threshold_x(image, ksize=3, thresh=(20, 100))
#image = threshold_mag(image, ksize=3, thresh=(40, 100))
#image = threshold_dir(image, ksize=3, thresh=(0.7, 1.3))

image = threshold_hls(image, thresh=(80, 255))

plt.imshow(image, cmap='gray');

def threshold_image(image):
    
    image = undistort(image)
    image = gaussian_blur(image, ksize=5)
    
    # Threshold Gradient
    binary_x = threshold_x(image, ksize=3, thresh=(20, 100))
    binary_mag = threshold_mag(image, ksize=3, thresh=(40, 100))
    binary_dir = threshold_dir(image, ksize=15, thresh=(0.7, 1.3)) 
    
    # Threshold HLS
    binary_hls_s = threshold_hls(image, thresh=(80, 255))
    
    # Combine two binary thresholds
    q = (binary_x==1) | (binary_hls_s==1)
    binary_combined_1 = np.zeros_like(binary_x)
    binary_combined_1[q] = 1
    
    # Any One
    q = (binary_x==1) | (binary_mag==1) | (binary_hls_s==1)
    binary_combined_2 = np.zeros_like(binary_x)
    binary_combined_2[q] = 1  
    
    
    # Any Two
    '''
    binary_combined_3 = np.zeros_like(binary_x)
    opt1 = (binary_x==1) & (binary_hls_s==1)
    opt2 = (sx_binary == 1) & (R_binary == 1)
    opt3 = (s_binary == 1) & (R_binary == 1)
    opt = opt1 | opt2 | opt3
    combined_binary[opt] = 1 
    '''
    
    # Perspective transform
    warped, unperspective, M, Minv = warp(binary_combined_1)
    
    #return image, binary_x, binary_mag, binary_dir, binary_hls_s, binary_combined_1, binary_combined_2, warped, Minv 
    return image, binary_x, binary_hls_s, binary_combined_1, warped, Minv

image = images[3]
image = mpimg.imread(image)

image, binary_x, binary_hls_s, binary_combined_1, warped, Minv = threshold_image(image)

plt.imshow(binary_combined_1, cmap='gray');

plt.savefig('./data/output_images/sample_image_thresholded.jpg') 

Sample Test Image - Threshold

image = images[3]
original = mpimg.imread(image)

image, binary_x, binary_hls_s, binary_combined_1, warped, Minv = threshold_image(original)

imgs1, imgs2, imgs3, imgs4, imgs5 = [image], [binary_x], [binary_hls_s], [binary_combined_1], [warped]

original = [original]

plot_5_images(original, imgs1, imgs2, imgs3, imgs4, imgs5, 'original', 'Undistort', 'threshold_x', 'threshold_hls_s', 'combined1_x_hls_s', 'warped')

plt.savefig('./data/output_images/sample_image_thresholded_warped.jpg') 

Test Images - Threshold

images = glob.glob('./data/test_images/*.jpg')

test_originals, test_undistort, test_binary_x, test_binary_hls_s, test_binary_combined_1, test_warped, test_Minv = [], [], [], [], [], [], []

for image in images:
    
    original = mpimg.imread(image)
    
    ret = threshold_image(original)
    
    image_undist, binary_x, binary_hls_s, binary_combined_1, warped, Minv = ret
    
    test_originals.append(original)
    test_undistort.append(image_undist)
    test_binary_x.append(binary_x)
    test_binary_hls_s.append(binary_hls_s)
    test_binary_combined_1.append(binary_combined_1)
    test_warped.append(warped)
    test_Minv.append(Minv)

plot_5_images(test_originals, test_undistort, test_binary_x, test_binary_hls_s, test_binary_combined_1, test_warped, 'original', 'Undistort', 'threshold_x', 'threshold_hls_s', 'combined1_x_hls_s', 'warped', (20, 25))

plt.savefig('./data/output_images/test_images_thresholded_warped.jpg') 

Fit Lines

def find_lane_pixels(binary_warped):
    image = binary_warped.copy()
    
    # Histogram of the bottom half of the image
    h, w = image.shape[0], image.shape[1]    
    half_image = image[h//2:, :]
    histogram = np.sum(half_image, axis=0)
    
    #print(w, h, histogram.shape)
    
    # Output image to draw on and  visualize the result
    out_img = np.dstack((image, image, image)) * 255 
    
    # histogram peaks and left/right lanes
    mid = histogram.shape[0] // 2 
    leftx_base = np.argmax(histogram[150:mid]) + 150 # No need to start search from 0
    rightx_base = np.argmax(histogram[mid+200:-100]) + mid+200 #  # No need to go beyond 

    # Hyperparameters
    nwindows = 9 
    margin = 50 
    minpix = 50  
    window_height = h // nwindows
    
    # x and y positions of all nonzero pixels
    nonzero = image.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])

    # Current Positions to be updated
    leftx_current = leftx_base
    rightx_current = rightx_base
    
    # left and right lane pixel indices
    left_lane_inds = []
    right_lane_inds = []
    
    for window in range(nwindows):
        # Identify window boundaries in x and y
        
        win_y_high = h - window * window_height
        win_y_low = h - (window + 1) * window_height
        
        win_xleft_low = leftx_current - margin
        win_xleft_high = leftx_current + margin
        
        win_xright_low = rightx_current - margin
        win_xright_high = rightx_current + margin
                
        
        # Draw the windows on the visualization image
        pt1 = (win_xleft_low, win_y_low)
        pt2 = (win_xleft_high, win_y_high)
        color = (0, 255, 0)
        thickness = 2
        cv2.rectangle(out_img, pt1, pt2, color, thickness)
        
        pt1 = (win_xright_low, win_y_low)
        pt2 = (win_xright_high, win_y_high)
        cv2.rectangle(out_img, pt1, pt2, color, thickness)

        
        # Identify the nonzero pixels in x and y within the window
        left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & \
                    (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high))
        good_left_inds = left_inds.nonzero()[0]

        right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & \
                    (nonzerox >= win_xright_low) & (nonzerox < win_xright_high))
        good_right_inds = right_inds.nonzero()[0]
                
        # Append these indices to the lists
        left_lane_inds.append(good_left_inds)
        right_lane_inds.append(good_right_inds)

        # If found > minpix pixels, recenter next window on their mean position
        if len(good_left_inds) > minpix:
            inds = nonzerox[good_left_inds]
            leftx_current = np.int(np.mean(inds))
            
        if len(good_right_inds) > minpix:
            inds = nonzerox[good_right_inds]
            rightx_current = np.int(np.mean(inds))
       
        # Concatenate the arrays of indices (previously was a list of lists of pixels)
   
    left_lane_inds = np.concatenate(left_lane_inds)
    right_lane_inds = np.concatenate(right_lane_inds)
   
    # Extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    
    return leftx, lefty, rightx, righty, out_img

def fit_polynomial(binary_warped, plot=True):
    
    h, w = binary_warped.shape[0], binary_warped.shape[1]
    
    leftx, lefty, rightx, righty, out_img = find_lane_pixels(binary_warped)
    
    # Fit a second order polynomial to each using `np.polyfit`
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    
    # Generate x and y values for plotting
    ploty = np.linspace(0, h-1, h)
    
    left_fitx = left_fit[0] * ploty**2 + left_fit[1] * ploty + left_fit[2]
    right_fitx = right_fit[0] * ploty**2 + right_fit[1] * ploty + right_fit[2]
    
    # Colors in the left and right lane regions
    out_img[lefty, leftx] = [255, 0, 0]
    out_img[righty, rightx] = [0, 0, 255]
    
    # Plots the left and right polynomials on the lane lines
    if plot:
        plt.plot(left_fitx, ploty, color='yellow')
        plt.plot(right_fitx, ploty, color='yellow')
    
    return out_img, left_fit, right_fit, ploty, left_fitx, right_fitx 

plt.imshow(test_originals[4]);

out_img, left_fit, right_fit, ploty, left_fitx, right_fitx = fit_polynomial(test_warped[0], plot=False)
plt.imshow(out_img)
plt.plot(left_fitx, ploty, color='yellow')
plt.plot(right_fitx, ploty, color='yellow')

plt.title('Lane Finding using Sliding Window', fontsize=16);

plt.savefig('./data/output_images/sample_image_lane_sliding.jpg') 

test_fit_img = []

test_left_fit = []
test_right_fit = []

test_fit_y = []
test_left_fit_x = []
test_right_fit_x = []

for w_img in test_warped:
    out_img, left_fit, right_fit, plot_y, left_fitx, right_fitx = fit_polynomial(w_img, plot=False)
    
    test_fit_img.append(out_img)   

    test_left_fit.append(left_fit)
    test_right_fit.append(right_fit)
    
    test_fit_y.append(plot_y)
    test_left_fit_x.append(left_fitx)
    test_right_fit_x.append(right_fitx)
    

len(test_originals), len(test_fit_img), len(test_fit_y), len(test_left_fit_x), len(test_right_fit_x)

(8, 8, 8, 8, 8)

plot_pair_images_fit(test_originals, test_fit_img, 4, 2, (12, 6), 'original', 'warped', test_fit_y, test_left_fit_x, test_right_fit_x)

plt.savefig('./data/output_images/test_images_lane_sliding.jpg') 

Skip the sliding windows

def fit_poly(img_shape, leftx, lefty, rightx, righty):
    h, w = img_shape[0], img_shape[1]
    
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    
    ploty = np.linspace(0, h-1, h)
    
    left_fitx = left_fit[0] * ploty**2 + left_fit[1] * ploty + left_fit[2]
    right_fitx = right_fit[0] * ploty**2 + right_fit[1] * ploty + right_fit[2] 
    
    return left_fitx, right_fitx, ploty

def fit_poly_left(img_shape, leftx, lefty):
    h, w = img_shape[0], img_shape[1]
    
    left_fit = np.polyfit(lefty, leftx, 2)
    ploty = np.linspace(0, h-1, h)
    left_fitx = left_fit[0] * ploty**2 + left_fit[1] * ploty + left_fit[2]
    
    return left_fit, left_fitx, ploty

def fit_poly_right(img_shape, rightx, righty):
    h, w = img_shape[0], img_shape[1]
    
    right_fit = np.polyfit(righty, rightx, 2)
    
    ploty = np.linspace(0, h-1, h)
    right_fitx = right_fit[0] * ploty**2 + right_fit[1] * ploty + right_fit[2] 
    
    return right_fit, right_fitx, ploty

def search_around_poly(binary_warped, left_fit, right_fit, disp_fit=True):
    
    margin = 50   
    
    # Grab Activate Pixels
    nonzero = binary_warped.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    
    xvals = left_fit[0] * nonzeroy**2 + left_fit[1] * nonzeroy + left_fit[2]
    left_lane_inds = (nonzerox > xvals - margin) & (nonzerox < xvals + margin)

    xvals = right_fit[0] * nonzeroy**2 + right_fit[1] * nonzeroy + right_fit[2]
    right_lane_inds = (nonzerox > xvals - margin) & (nonzerox < xvals + margin)
    
    left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + 
                    left_fit[2] - margin)) & (nonzerox < (left_fit[0]*(nonzeroy**2) + 
                    left_fit[1]*nonzeroy + left_fit[2] + margin)))
                    
    right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + 
                    right_fit[2] - margin)) & (nonzerox < (right_fit[0]*(nonzeroy**2) + 
                    right_fit[1]*nonzeroy + right_fit[2] + margin)))
    

    # extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    
    # Fit new polynomials
    '''
    if len(leftx) != 0 and len(rightx) != 0:
        left_fitx, right_fitx, ploty = fit_poly(binary_warped.shape, leftx, lefty, rightx, righty)
    '''
    left_fit, left_fitx, ploty = None, None, None
    right_fit, right_fitx, ploty = None, None, None
    
    if len(leftx) != 0:
        left_fit, left_fitx, ploty = fit_poly_left(binary_warped.shape, leftx, lefty)
        
    if len(rightx) != 0:
        right_fit, right_fitx, ploty = fit_poly_right(binary_warped.shape, rightx, righty)
        
    if len(leftx) != 0 and len(rightx) != 0:    
    
    ## Visualization ##
    
    # Create an image to draw on and an image to show the selection window
        out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
    
        window_img = np.zeros_like(out_img)
    
        # Color in left and right line pixels
        out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
        out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]

        # Generate a polygon to illustrate the search window area
        # And recast the x and y points into usable format for cv2.fillPoly()
        left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, ploty]))])
        left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin, 
                              ploty])))])
        
        left_line_pts = np.hstack((left_line_window1, left_line_window2))
        right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, ploty]))])
        right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin, 
                              ploty])))])
        right_line_pts = np.hstack((right_line_window1, right_line_window2))

        # Draw the lane onto the warped blank image
        cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0))
        cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0))
        
        result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)
    
        # Plot the polynomial lines onto the image
        if disp_fit and (len(leftx) != 0 and len(rightx) != 0):
            plt.plot(left_fitx, ploty, color='yellow')
            plt.plot(right_fitx, ploty, color='yellow')
        ## End visualization steps ##

        return result, left_fit, right_fit, left_fitx, right_fitx, ploty
    
    else:
        out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
        window_img = np.zeros_like(out_img)
        result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)
        left_fitx, right_fitx, ploty = [], [], []
        
        return result, left_fit, right_fit, left_fitx, right_fitx, ploty

result, left_fit, right_fit, left_fitx, right_fitx, ploty = search_around_poly(test_warped[0], test_left_fit[0], test_right_fit[0], disp_fit=False)

# View your output
plt.imshow(result);
plt.plot(left_fitx, ploty, color='yellow')
plt.plot(right_fitx, ploty, color='yellow')

plt.title('Sample Lane', fontsize=20)
#plt.savefig('./output_images/sample_lane.png')

plt.savefig('./data/output_images/sample_image_lane_skip_sliding.jpg');

test_fit_img = []

test_fit_y = []

test_left_fit_x = []
test_right_fit_x = []

for idx, w_img in enumerate(test_warped):
    out_img, left_fit, right_fit, left_fitx, right_fitx, ploty = search_around_poly(test_warped[idx], test_left_fit[idx], test_right_fit[idx], disp_fit=False)
    
    test_fit_img.append(out_img)   
    
    test_fit_y.append(plot_y)
    
    test_left_fit_x.append(left_fitx)
    test_right_fit_x.append(right_fitx)

plot_pair_images_fit(test_originals, test_fit_img, 4, 2, (12, 6), 'original', 'warped', test_fit_y, test_left_fit_x, test_right_fit_x)

plot_pair_images_fit(test_originals, test_fit_img, 4, 2, (12, 6), 'original', 'warped')

plt.savefig('./data/output_images/test_images_lane_skip_sliding.jpg') 

Measure Curvature

def measure_curvature_pixels(left_fitx, right_fitx, ploty):
    
    left_fit = np.polyfit(ploty, left_fitx, 2)
    right_fit = np.polyfit(ploty, right_fitx, 2)
    
    y_eval = np.max(ploty)
        
    A, B, C = left_fit[0], left_fit[1], left_fit[2]
    
    left_curverad = (((1 + (2 * A * y_eval + B)**2))**1.5) / np.absolute(2*A)

    A, B, C = right_fit[0], right_fit[1], right_fit[2]
    right_curverad = (((1 + (2 * A * y_eval + B)**2))**1.5) / np.absolute(2*A)
        
    return left_curverad, right_curverad


def measure_curvature_real(left_fitx, right_fitx, ploty):
    '''
    Calculates the curvature of polynomial functions in meters.
    '''
    # Define conversions in x and y from pixels space to meters
    ym_per_pix = 30/720 # meters per pixel in y dimension
    xm_per_pix = 3.7/700 # meters per pixel in x dimension
    
    left_fitx = [lx * xm_per_pix for lx in left_fitx]
    right_fitx = [rx * xm_per_pix for rx in right_fitx]
    
    ploty = [py * ym_per_pix for py in ploty]
    
    left_fit = np.polyfit(ploty, left_fitx, 2)
    right_fit = np.polyfit(ploty, right_fitx, 2)

    y_eval = np.max(ploty)
    
    
    A, B, C = left_fit[0], left_fit[1], left_fit[2]
    
    y_eval = y_eval * ym_per_pix 
    
    left_curverad = (((1 + (2 * A * y_eval + B)**2))**1.5) / np.absolute(2*A)
    
    A, B, C = right_fit[0], right_fit[1], right_fit[2]
    
    right_curverad = (((1 + (2 * A * y_eval + B)**2))**1.5) / np.absolute(2*A)
    
    avg_curvature = (left_curverad + right_curverad)//2 
        
    return avg_curvature

def get_centre(left_fitx, right_fitx, ploty, image=image):
    
    left_fitx = left_fitx[-20:]
    right_fitx = right_fitx[-20:] 

    xm_per_pix = 3.7/700 # meters per pixel in x dimension
    
    diff = [l + (r - l)/2 for r, l in zip(right_fitx, left_fitx)]
    vehicle_pos = np.mean(diff)

    h, w = image.shape[0], image.shape[1]
    
    centre_image = w // 2 
    
    offset = centre_image - vehicle_pos
    offset = offset * xm_per_pix
    offset = round(offset, 2)
    
    if offset > 0:
        msg = f'Vehicle is {np.absolute(offset)}m left of center'
    if offset < 0:
        msg = f'Vehicle is {np.absolute(offset)}m right of center'
    if offset == 0:
        msg = f'Vehicle is {np.absolute(offset)}m at center'
    
    return offset, msg

for idx in range(len(test_fit_img)):
    
    avg_curvature = measure_curvature_real(test_left_fit_x[idx], test_right_fit_x[idx], test_fit_y[idx])

    cx, msg2 = get_centre(test_left_fit_x[idx], test_right_fit_x[idx], test_fit_y[idx], test_originals[idx])

    msg1 = f'Radius of Curvature = {avg_curvature}(m)'
    font = cv2.FONT_HERSHEY_SIMPLEX

    if len(test_left_fit_x[idx]) != 0 and len(test_right_fit_x[idx]) != 0:
        cv2.putText(test_fit_img[idx], msg1, (50, 50), font, 1.2, (255,255,255), 5)
        cv2.putText(test_fit_img[idx], msg2, (50, 100), font, 1.2, (255,255,255), 5)

plot_pair_images(test_originals, test_fit_img, 4, 2, (15, 10), 'Original', 'Output' )

plt.savefig('./data/output_images/test_images_lane_skip_sliding_cc.jpg') 

Sample Output

test_results = []
for idx in range(len(test_fit_img)):
    idx = 2
    w, h = test_fit_img[idx].shape[1], test_fit_img[idx].shape[0]
    
    out_image = cv2.warpPerspective(test_fit_img[idx], test_Minv[idx], (w, h))

    result = cv2.addWeighted(test_originals[idx], 1, out_image, 0.3, 0)

    test_results.append(result)

plot_pair_images(test_originals, test_results, 4, 2, (15, 10), 'Original', 'Output' )

plt.savefig('./data/output_images/test_images_lanes.jpg') 

Process Image

def sanity_check(left_fit, right_fit):
    status = True
    if len(left_fit) ==0 or len(right_fit) == 0:
        status = False
        
    return status

def get_points(h, left_fit, right_fit):
    ploty = np.linspace(0, h-1, h)
    left_fitx = left_fit[0] * ploty**2 + left_fit[1] * ploty + left_fit[2]
    right_fitx = right_fit[0] * ploty**2 + right_fit[1] * ploty + right_fit[2] 
    return ploty, left_fitx, right_fitx

def process_image(im):
    global count 
    global left_fit
    global right_fit  
    global last_left_fit
    global last_right_fit 
    global last_left_fitx
    global last_right_fitx
    
    w, h = im.shape[1], im.shape[0]
    
    im_undist, binary_x, binary_hls_s, binary_combined_1, warped, Minv = threshold_image(im)
    
    if count == 0:
        _, left_fit, right_fit, _, _, _ = fit_polynomial(warped, plot=False)
    
    out_image, left_fit, right_fit, left_fitx, right_fitx, ploty = search_around_poly(warped, left_fit, right_fit, disp_fit=False)
        
    status = sanity_check(left_fit, right_fit)
    if status:
        last_left_fit, last_right_fit = left_fit, right_fit
        last_left_fitx, last_right_fitx, last_ploty = left_fitx, right_fitx, ploty
        count += 1
    else:
        left_fit, right_fit = last_left_fit, last_right_fit
        left_fitx, right_fitx, ploty = last_left_fitx, last_right_fitx, last_ploty
        
        #left_fitx, right_fitx, ploty = get_points(h, left_fit, right_fit)
        
    for x1, x2, y1 in zip(left_fitx, right_fitx, ploty):
        x1, x2, y1 = int(x1), int(x2), int(y1)
        y2 = y1
        cv2.line(out_image, (x1, y1), (x2, y2), (0,255,0), 2)
    
    out_image = cv2.warpPerspective(out_image, Minv, (w, h))
    
    result = cv2.addWeighted(im, 1, out_image, 0.3, 0)
    
    '''
    if len(left_fitx) != 0 and len(right_fitx) != 0:
        last_left_fitx = left_fitx
        last_right_fitx = right_fitx 
        
    ''' 
    if status:  
        avg_curvature = measure_curvature_real(left_fitx, right_fitx, ploty)
            
        cx, msg2 = get_centre(left_fitx, right_fitx, ploty, im)  

        msg1 = f'Radius of Curvature = {avg_curvature}(m)'
        
        font = cv2.FONT_HERSHEY_SIMPLEX
        
        cv2.putText(result, msg1, (50,50), font, 1.2, (255,255,255), 5)
        cv2.putText(result, msg2, (50, 100), font, 1.2, (255,255,255), 5)
        
    
    return result

count = 0 
inp = test_originals[4]
result = process_image(inp)
plt.imshow(result)
plt.title('Sample Output', fontsize=20);

plt.savefig('./data/output_images/sample_image_output.png') 

results = []
for inp in test_originals:
    count = 0 
    results.append(process_image(inp))

plot_pair_images(test_originals, results, 4, 2, (15, 10), 'original', 'Output')

plt.savefig('./data/output_images/test_images_output.png') 

Process Video

# Need to Manually Remove first in case output not correct
vid_output = './data/output_images/project_video_marked.mp4'

if os.path.exists(vid_output):
    os.remove(vid_output)

import time
time.sleep(30) # Ensure OS Delay that file is removed 

count = 0

clip = VideoFileClip('./data/project_video.mp4')

clip_marked = clip.fl_image(lambda image: process_image(image))
clip_marked.write_videofile(vid_output, audio=False)

t:   0%|          | 2/1260 [00:00<01:23, 15.11it/s, now=None]

Moviepy - Building video ./data/output_images/project_video_marked.mp4.
Moviepy - Writing video ./data/output_images/project_video_marked.mp4

Moviepy - Done !
Moviepy - video ready ./data/output_images/project_video_marked.mp4

print('complete')

complete